
#include "sal_types.h"
#include "sal.h"
#include "ctc_const.h"
#include "ctc_macro.h"




#define CTC_BMP_OP_RANGE_ONE_BITDCL(_a1,_a2, _offs, _n, _dest, _op)     \
    {                                                                   \
        uint32 _mask = ~0;                                          \
        uint32 _data;                                               \
                                                                        \
        _mask >>= (CTC_UINT32_BITS - (_n));                                  \
        _mask <<=_offs;                                                 \
        _data = ((_a1) _op (_a2)) & _mask;                              \
        *(_dest) &= ~_mask;                                             \
        *(_dest) |= _data;                                              \
    }

/*!
 * \brief Perform bitwise logical operation on bit arrays.
 *
 * This macro allows code sharing between different types of bitwise
 * logical operations between two bit arrays.
 *
 * \param [in] _a1 First bit array for operation
 * \param [in] _a2 Second bit array for operation
 * \param [in] _offs Offset (in bits) into the arrays
 * \param [in] _n Number of bits to operate on
 * \param [in] _dest Destination bit array
 * \param [in] _op Type operation (applied in between BITDCLs)
 *
 * \return Nothing
 */
#define CTC_BMP_OP_RANGE(_a1, _a2, _offs, _n, _dest, _op)               \
    {                                                                   \
        const uint32 *_ptr_a1;                                      \
        const uint32 *_ptr_a2;                                      \
        uint32 *_ptr_dest;                                          \
        int32 _woffs, _wremain;                                           \
                                                                        \
        _ptr_a1 = (_a1) + ((_offs) / CTC_UINT32_BITS);                       \
        _ptr_a2 = (_a2) + ((_offs) / CTC_UINT32_BITS);                       \
        _ptr_dest = (_dest) + ((_offs) / CTC_UINT32_BITS);                   \
        _woffs = ((_offs) % CTC_UINT32_BITS);                                \
                                                                        \
        _wremain = CTC_UINT32_BITS - _woffs;                                 \
        if ((_n) <= _wremain) {                                         \
            CTC_BMP_OP_RANGE_ONE_BITDCL(*_ptr_a1, *_ptr_a2,             \
                                        _woffs, (_n),                   \
                                        _ptr_dest, _op);                \
            return;                                                     \
        }                                                               \
        CTC_BMP_OP_RANGE_ONE_BITDCL(*_ptr_a1, *_ptr_a2,                 \
                                    _woffs, _wremain,                   \
                                    _ptr_dest, _op);                    \
        (_n) -= _wremain;                                               \
        ++_ptr_a1; ++_ptr_a2; ++_ptr_dest;                              \
        while ((_n) >= CTC_UINT32_BITS) {                                    \
            *_ptr_dest =                                                \
                (*_ptr_a1) _op (*_ptr_a2);                              \
            (_n) -= CTC_UINT32_BITS;                                         \
            ++_ptr_a1; ++_ptr_a2; ++_ptr_dest;                          \
        }                                                               \
        if ((_n) > 0) {                                                 \
            CTC_BMP_OP_RANGE_ONE_BITDCL(*_ptr_a1, *_ptr_a2,             \
                                        0, (_n),                        \
                                        _ptr_dest, _op);                \
        }                                                               \
    }





STATIC INLINE void
ctc_bit_range_clear_one_bitdcl(uint32 *a, int32 offs, int32 n)
{
    uint32 mask = ~0;

    mask >>= (CTC_UINT32_BITS - n);
    mask <<= offs;
    *a &= ~mask;
}


void
ctc_bit_range_clear(uint32 *a, int32 offs, int32 n)
{
    uint32 *pa;
    int32 woffs, wremain;

    if (n <= 0)
    {
        return;
    }

    pa = a + (offs / CTC_UINT32_BITS);

    woffs = offs % CTC_UINT32_BITS;

    if (woffs != 0)
    {
        wremain = CTC_UINT32_BITS - woffs;
        if (n <= wremain)
        {
            ctc_bit_range_clear_one_bitdcl(pa, woffs, n);
            return;
        }
        ctc_bit_range_clear_one_bitdcl(pa, woffs, wremain);
        n -= wremain;
        ++pa;
    }
    while (n >= CTC_UINT32_BITS)
    {
        *(pa++) = 0;
        n -= CTC_UINT32_BITS;
    }

    if (n > 0)
    {
        ctc_bit_range_clear_one_bitdcl(pa, 0, n);
    }
}



STATIC INLINE int32
ctc_bit_popcount32(uint32 w)
{
    w = (w & 0x55555555) + ((w >> 1) & 0x55555555);
    w = (w & 0x33333333) + ((w >> 2) & 0x33333333);
    w = (w + (w >> 4)) & 0x0f0f0f0f;
    w = w + (w >> 8);

    return (w + (w >> 16)) & 0xff;
}


STATIC INLINE int32
ctc_bit_range_count_one_bitdcl(uint32 b, int32 offs, int32 n)
{
    uint32 mask;
    int32 count = 0;
    uint32 i = 0;

    if (n >= 0)
    {
        mask = ~0;
        mask >>= (CTC_UINT32_BITS - n);
        mask <<= offs;
        b &= mask;
    }

    do
    {
        count += ctc_bit_popcount32(b >> (i * sizeof(uint32) * 8));
    }
    while (++i < sizeof(uint32) / sizeof(uint32));

    return count;
}


void
ctc_bit_range_count(const uint32 *bits, int32 offs, int32 n, int32 *count)
{
    const uint32 *ptr;
    int32 woffs, wremain;

    *count = 0;

    if (n <= 0)
    {
        return;
    }

    ptr = bits + (offs / CTC_UINT32_BITS);
    woffs = offs % CTC_UINT32_BITS;

    if (woffs != 0)
    {
        wremain = CTC_UINT32_BITS - woffs;
        if (n <= wremain)
        {
            *count = ctc_bit_range_count_one_bitdcl(*ptr, woffs, n);
            return;
        }
        *count += ctc_bit_range_count_one_bitdcl(*ptr, woffs, wremain);
        n -= wremain;
        ++ptr;
    }
    while (n >= CTC_UINT32_BITS)
    {
        *count += ctc_bit_range_count_one_bitdcl(*(ptr++), 0, -1);
        n -= CTC_UINT32_BITS;
    }
    if (n > 0)
    {
        *count += ctc_bit_range_count_one_bitdcl(*ptr, 0, n);
    }
}






STATIC INLINE int32
ctc_bit_range_eq_one_bitdcl(uint32 b1, uint32 b2, int32 offs, int32 n)
{
    uint32 mask = ~0;

    mask >>= (CTC_UINT32_BITS - n);
    /* Move the mask to start from 'offs' offset */
    mask <<= offs;

    return ((b1 & mask) == (b2 & mask));
}

/*compare a1 and a2 in n bits from offs*/
int32
ctc_bit_range_eq(const uint32 *a1, const uint32 *a2,
                   int32 offs, int32 n)
{
    const uint32 *ptr1;
    const uint32 *ptr2;
    int32 woffs, wremain;

    if (n <= 0) {
        return 1;
    }

    ptr1 = a1 + (offs / CTC_UINT32_BITS);
    ptr2 = a2 + (offs / CTC_UINT32_BITS);

    woffs = offs % CTC_UINT32_BITS;

    if (woffs != 0) {
        wremain = CTC_UINT32_BITS - woffs;
        if (n <= wremain) {
            return ctc_bit_range_eq_one_bitdcl(*ptr1, *ptr2, woffs, n);
        }
        if (!ctc_bit_range_eq_one_bitdcl(*ptr1, *ptr2, woffs, wremain)) {
            return 0;
        }
        n -= wremain;
        ++ptr1, ++ptr2;
    }
    while (n >= CTC_UINT32_BITS) {
        if (*(ptr1++) != *(ptr2++)) {
            return 0;
        }
        n -= CTC_UINT32_BITS;
    }
    if (n > 0) {
        return ctc_bit_range_eq_one_bitdcl(*ptr1, *ptr2, 0, n);
    }
    return 1;
}




STATIC INLINE int32
ctc_bit_range_null_one_bitdcl(uint32 b, int32 offs, int32 n)
{
    uint32 mask = ~0;

    mask >>= (CTC_UINT32_BITS - n);
    /* Move the mask to start from 'offs' offset */
    mask <<= offs;

    return ((b & mask) == 0);
}


int32
ctc_bit_range_null(const uint32 *a, int32 offs, int32 n)
{
    const uint32 *ptr;
    int32 woffs, wremain;

    if (n <= 0) {
        return 1;
    }

    /* Pointer to offs uint32 in 'a' that contains 'offs' */
    ptr = a + (offs / CTC_UINT32_BITS);

    /* Offset of 'offs' bit within this SDH_BITDCL */
    woffs = offs % CTC_UINT32_BITS;

    /* Check if 'offs' is CTC_UINT32_BITS aligned */
    if (woffs != 0) {
        /*  Get remaining bits in this SDH_BITDCL */
        wremain = CTC_UINT32_BITS - woffs;
        if (n <= wremain) {
            /* All the range is in one uint32 */
            return ctc_bit_range_null_one_bitdcl(*ptr,
                                                   woffs,
                                                   n);
        }
        /* We should check the offs uint32, and might also continue */
        if (!ctc_bit_range_null_one_bitdcl(*ptr, woffs, wremain)) {
            return 0;
        }
        n -= wremain;
        ++ptr;
    }
    while (n >= CTC_UINT32_BITS) {
        /* We're testing a full uint32 */
        if (*(ptr++)) {
            return 0;
        }
        n -= CTC_UINT32_BITS;
    }
    /* This is the last uint32, and it is not CTC_UINT32_BITS */
    if (n > 0) {
        return ctc_bit_range_null_one_bitdcl(*ptr, 0, n);
    }
    return 1;
}


/*!
 * INTERNAL USE ONLY.
 *
 * Same as ctc_bit_range_negate, but for a single uint32.
 */
STATIC INLINE void
ctc_bit_negate_range_one_bitdcl(const uint32 *a,
                               int32 offs, int32 n, uint32 *dest)
{
    uint32 mask = ~0;
    uint32 data;

    mask >>= (CTC_UINT32_BITS - n);
    mask <<= offs;
    data =  ~*a & mask;
    *dest &= ~mask;
    *dest |= data;
}


void
ctc_bit_range_negate(const uint32 *a,
                       int32 offs, int32 n, uint32 *dest)
{
    const uint32 *pa;
    uint32 *pd;
    int32 woffs, wremain;

    if (n <= 0) {
        return;
    }

    pa = a + (offs / CTC_UINT32_BITS);
    pd = dest + (offs / CTC_UINT32_BITS);
    woffs = (offs % CTC_UINT32_BITS);

    wremain = CTC_UINT32_BITS - woffs;
    if (n <= wremain) {
        ctc_bit_negate_range_one_bitdcl(pa, woffs, n, pd);
        return;
    }
    ctc_bit_negate_range_one_bitdcl(pa, woffs, wremain, pd);
    n -= wremain;
    ++pa; ++pd;
    while (n >= CTC_UINT32_BITS) {
        *pd = ~(*pa);
        n -= CTC_UINT32_BITS;
        ++pa; ++pd;
    }
    if (n > 0) {
        ctc_bit_negate_range_one_bitdcl(pa, 0, n, pd);
    }
}



/*!
 * \brief Perform bitwise AND operation on bit arrays.
 *
 * See \ref CTC_BMP_AND_RANGE for details.
 */
void
ctc_bit_range_and(const uint32 *a1, const uint32 *a2,
                    const int32 offs, int32 n, uint32 *dest)
{
    if (n > 0) {
        CTC_BMP_OP_RANGE(a1, a2, offs, n, dest, &);
    }
}

/*!
 * \brief Perform bitwise OR operation on bit arrays.
 *
 * INTERNAL USE ONLY.
 *
 * See \ref CTC_BMP_OR_RANGE for details.
 */
void
ctc_bit_range_or(const uint32 *a1, const uint32 *a2,
                   const int32 offs, int32 n, uint32 *dest)
{
    if (n > 0) {
        CTC_BMP_OP_RANGE(a1, a2, offs, n, dest, |);
    }
}

/*!
 * \brief Perform bitwise XOR operation on bit arrays.
 *
 * INTERNAL USE ONLY.
 *
 * See \ref CTC_BMP_XOR_RANGE for details.
 */
void
ctc_bit_range_xor(const uint32 *a1, const uint32 *a2,
                    const int32 offs, int32 n, uint32 *dest)
{
    if (n > 0) {
        CTC_BMP_OP_RANGE(a1, a2, offs, n, dest, ^);
    }
}

/*
 * \brief Clear select bits in a bit array.
 *
 * INTERNAL USE ONLY.
 *
 * See \ref CTC_BMP_REMOVE_RANGE for details.
 */
void
ctc_bit_range_remove(const uint32 *a1, const uint32 *a2,
                       const int32 offs, int32 n, uint32 *dest)
{
    if (n > 0) {
        CTC_BMP_OP_RANGE(a1, a2, offs, n, dest, & ~);
    }
}

/*
 *
 * Same as ctc_bit_range_copy, but for a single uint32.
 */
STATIC INLINE void
ctc_bit_range_copy_one_bitdcl(uint32 *a1, int32 offs1, uint32 a2,
                                int32 offs2, int32 n)
{
    uint32 data;
    uint32 mask;

    /*
     * No need to check that offs1 == 0 and offs2 == 0, It
     * must be becuse of the constraints.
     */
    if (n == CTC_UINT32_BITS) {
        *(a1) = a2;
        return;
    }

    /* Get the data */
    data = a2 >> offs2;

    /* Align the data to the place it may be inserted */
    data <<= offs1;

    /*
     * We might have bits in a2 above offs2 + n that need
     * to be cleared.
     */
    mask = ~0;
    mask >>= CTC_UINT32_BITS - n;
    mask <<= offs1;
    data &= mask;
    *a1 &= ~mask;
    *a1 |= data;
}

/*
 * \brief Copy a range of bits between two bit arrays.
 *
 *
 * Refer to \ref CTC_BMP_COPY_RANGE macro.
 */
void
ctc_bit_range_copy(uint32 *a1, int32 offs1, const uint32 *a2,
                     int32 offs2, int32 n)
{
    uint32 *cur_dst;
    const uint32 *cur_src;
    int32 woff_src, woff_dst, wremain;

    if (n <= 0) {
        return;
    }

    if ((offs1 % CTC_UINT32_BITS) == 0 &&
        (offs2 % CTC_UINT32_BITS) == 0 &&
        (n % CTC_UINT32_BITS) == 0) {
        /* Both source, destination and size are uint32 aligned */
        sal_memcpy(&(a1[offs1 / CTC_UINT32_BITS]), &(a2[offs2 / CTC_UINT32_BITS]),
                   CTC_BMP_ALLOCSIZE(n));
        return;
    }

    cur_dst = a1 + (offs1 / CTC_UINT32_BITS);
    cur_src = a2 + (offs2 / CTC_UINT32_BITS);

    woff_src = offs2 % CTC_UINT32_BITS;
    woff_dst = offs1 % CTC_UINT32_BITS;

    if (woff_dst >= woff_src) {
        wremain = CTC_UINT32_BITS - woff_dst;
    } else {
        wremain = CTC_UINT32_BITS - woff_src;
    }
    if (n <= wremain) {
        ctc_bit_range_copy_one_bitdcl(cur_dst, woff_dst,
                                        *cur_src, woff_src, n);
        return;
    }
    ctc_bit_range_copy_one_bitdcl(cur_dst, woff_dst,
                                    *cur_src, woff_src, wremain);
    n -= wremain;
    while (n >= CTC_UINT32_BITS) {
        if (woff_dst >= woff_src) {
            ++cur_dst;
            wremain = woff_dst - woff_src;
            if (wremain > 0) {
                ctc_bit_range_copy_one_bitdcl(cur_dst, 0, *cur_src,
                                                CTC_UINT32_BITS - wremain, wremain);
            }
        } else {
            ++cur_src;
            wremain = woff_src - woff_dst;
            ctc_bit_range_copy_one_bitdcl(cur_dst, CTC_UINT32_BITS - wremain,
                                            *cur_src, 0, wremain);
        }
        n -= wremain;
        wremain = CTC_UINT32_BITS - wremain;
        if (woff_dst >= woff_src) {
            ++cur_src;
            ctc_bit_range_copy_one_bitdcl(cur_dst, CTC_UINT32_BITS - wremain,
                                            *cur_src, 0, wremain);
        } else {
            ++cur_dst;
            ctc_bit_range_copy_one_bitdcl(cur_dst, 0, *cur_src,
                                            CTC_UINT32_BITS - wremain, wremain);
        }
        n -= wremain;
    }

    if (woff_dst >= woff_src) {
        ++cur_dst;
        wremain = woff_dst - woff_src;
        if (n <= wremain) {
            if (n > 0) {
                ctc_bit_range_copy_one_bitdcl(cur_dst, 0, *cur_src,
                                                CTC_UINT32_BITS - wremain, n);
            }
            return;
        }
        if (wremain > 0) {
            ctc_bit_range_copy_one_bitdcl(cur_dst, 0, *cur_src,
                                            CTC_UINT32_BITS - wremain, wremain);
        }
    } else {
        ++cur_src;
        wremain = woff_src - woff_dst;
        if (n <= wremain) {
            if (n > 0) {
                ctc_bit_range_copy_one_bitdcl(cur_dst, CTC_UINT32_BITS - wremain,
                                                *cur_src, 0, n);
            }
            return;
        }
        ctc_bit_range_copy_one_bitdcl(cur_dst, CTC_UINT32_BITS - wremain,
                                        *cur_src, 0, wremain);
    }
    n -= wremain;

    if (n > 0) {
        wremain = CTC_UINT32_BITS - wremain;
        if (woff_dst >= woff_src) {
            ++cur_src;
            ctc_bit_range_copy_one_bitdcl(cur_dst, CTC_UINT32_BITS - wremain,
                                            *cur_src, 0, n);
        } else {
            ++cur_dst;
            ctc_bit_range_copy_one_bitdcl(cur_dst, 0, *cur_src,
                                            CTC_UINT32_BITS - wremain, n);
        }
    }
}



/*
 *
 * Same as ctc_bit_range_set, but for a single uint32.
 */
STATIC INLINE void
ctc_bit_range_set_one_bitdcl(uint32 *a, int32 offs, int32 n)
{
    uint32 mask = ~0;
    mask >>= (CTC_UINT32_BITS - n);
    mask <<= offs;
    *a |= mask;
}

/*
 * \brief Set range of bits in a bit array.
 * *
 * Refer to \ref CTC_BMP_CLR_RANGE macro.
 */
void
ctc_bit_range_set(uint32 *a, int32 offs, int32 n)
{
    uint32 *pa;
    int32 woffs, wremain;

    if (n <= 0)
    {
        return;
    }

    pa = a + (offs / CTC_UINT32_BITS);

    woffs = offs % CTC_UINT32_BITS;

    if (woffs != 0)
    {
        wremain = CTC_UINT32_BITS - woffs;
        if (n <= wremain)
        {
            ctc_bit_range_set_one_bitdcl(pa, woffs, n);
            return;
        }
        ctc_bit_range_set_one_bitdcl(pa, woffs, wremain);
        n -= wremain;
        ++pa;
    }
    while (n >= CTC_UINT32_BITS)
    {
        *(pa++) = ~0;
        n -= CTC_UINT32_BITS;
    }
    if (n > 0)
    {
        ctc_bit_range_set_one_bitdcl(pa, 0, n);
    }
}




